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[57] ABSTRACT 

A multiple arm generahzed compliant motion robot 
control system governs dual multi-joint robot arms 
handling an object with both of the arms in accordance 
with input parameters governing plural respective be- 
haviors to be exhibited by the robot in respective behav- 
ior spaces simultaneously. A move-squeeze decomposi- 
tion processor computes actual move and squeeze de- 
composition forces based upon current robot force sen- 
sor outputs. A compliant motion processor transforms 
plural object position perturbations of the plural behav- 
iors from the respective behavior spaces to a common 
space, and computes a relative transformation to a 
behavior-commanded object position in accordance 
with the object position perturbations of the plural be- 
haviors. A kinematics processor updates a transforma- 
tion to a current commanded object position based upon 
the relative transformation to the behavior-commanded 
object position. A multiple arm squeeze control proces- 
sor computes, from appropriate squeeze force input 
parameters and from actual squeeze forces for each of 
the arms, a squeeze control position perturbation for 
each of the arms, to provide squeeze control. An in- 
verse kinematics processor computes, from the com- 
manded object position transformation and from the 
squeeze control position perturbation, new robot joint 
angles, and controls respective joints of the robot arms 
in accordance with the new robot joint angles. 

44 Claims, 16 Drawing Sheets 
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DUAL-ABM GENERALIZED COMPLIANT 
MOTION WITH SHARED CONTROL 

BACKGROUND OF THE INVENTION 5 
Origin of the Invention 

The invention described herein was made in the per- 
formance of work under a NASA contract, and is sub- 
ject to the provisions of Public Law 96-517 (35 USC jq 
202) in which the contractor has elected not to retain 
title. 

Technical Field 

The invention relates to dual arm robot control sys- 
terns and in particular to systems for controlling robots 
grasping a single object between two arms for perform- 
ing specific tasks in response to multiple user or sensor 
inputs. 

Background Art 20 

Multiple-arm robotic systems provide many valuable 
capabilities beyond single-arm systems. Coordinated 
control of multiple robots allows one robot to act as a 
flexible fixtiuing device while another robot executes a 
task. Cooperative control of multiple robots allows " 
multiple robots to cooperatively execute a task with 
common closed chain control. This may be valuable 
when handling heavy or extended objects or objects 
which are difficult to grasp securely with one arm. 

The invention described in this specification was 
motivated by the needs and constraints of space telero- 
botics. Dual-arm cooperative control may be valuable 
for manipulation objects which are extended or have 
large mass relative to the manipulators. Also, tasks in- 
volving relatively small objects may also be more effec- 
tively executed using cooperative dual-arm control, as 
has been demonstrated using the methods described in 
this specification for the fluid coupler mating task of 
satellite servicing. There are many constraints for a 
space telerobotics control system such as limited com- ^ 
putational power, relatively fixed remote site software 
environment, and safety concerns. 

References 

The invention will be described below with reference 
to the following publications by number in square 
brackets, as in “[!”]: 

[1.] Paul G. Backes, Kam S. Tso, Thomas S. Lee, and 
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SUMMARY OF THE DISCLOSURE 

A dual-arm task execution primitive is provided for 
cooperative dual-arm telerobotic task execution utiliz- 
ing multiple sensors concurrently. The primitive has 
been integrated into a telerobot task execution system 
and can be called by a task planning system for execu- 
tion of tasks requiring dual-arm sensor based motion, 
e.g., force control, teleoperation and shared control. 
The primitive has a large input parameter set which is 
used to specify the desired behavior of the motion. 
Move-squeeze decomposition is utilized to decompose 
forces sensed at the wrists of the two manipulators into 
forces in the move subspace, which cause system mo- 
tion, and forces in the squeeze subspace, which cause 
internal forces. The move and squeeze forces are then 
separately controlled. Several space servicing tasks 
utilizing the cooperative dual-arm control capability are 


[5.] John T. Wen and Kenneth Kreutz, “Stability described and experimental results from the tasks are 
analysis of multiple rigid robot manipulators holding a given. The supervisory and shared control tasks include 
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capture of a rotating satellite, orbital replacement unit 
changeout, fluid coupler seating and locking, and con- 
tour following. 

The invention includes a unified control algorithm 
integrating cooperative dual-arm control with multi- 5 
sensor based task control. The invention employs vari- 
ous methods for cooperative dual-arm control, multi- 
sensor based control, task execution, task description 
and safety monitoring. The complete control capability 
is made available to a higher level task planning system 10 
as a task execution primitive with a large input parame- 
ter set which is used to describe the desired behavior of 
motion. The task primitive, called the Dual-Arm Gen- 
eralized Compliant Motion (DAGCM) primitive, pro- 
vides autonomous, teleoperation, and shared control 15 
capability of task execution as specified for the common 
object held by the two manipulators. Separate parame- 
terization are used to describe the control in the move 
and squeeze subspaces. A single-arm version of the 
primitive has been integrated into the local-remote 20 
telerobot system in the JPL Supervisory Telerobotics 
(STELER) Laboratory [1,2]. The autonomous, teleop- 
eration, and shared control capabilities are provided to 
an operator using the User Macro Interface [3]. Several 
space servicing tasks utilizing the primitive are de- 25 
scribed and experimental results from the tasks are 
given. 

In accordance with the invention, a multiple arm 
generalized compliant motion robot control system 
governs dual multi-joint robot arms handling an object 30 
with both of the arms. Initially, plural input parameters 
are defined as governing plural respective behaviors to 
be exhibited by the robot in respective behavior spaces 
simultaneously. Kinematic quantities such as position 
and force are defined as six-vectors, wherein “force” 35 
includes a three-vector force and a three-vector torque 
and “position” includes a three-vector location and a 
three-vector rotation orientation. A move-squeeze de- 
composition processor computes actual move and 
squeeze decomposition forces based upon current robot 40 
force sensor outputs. A compliant motion processor 
transforms plural object position perturbations of the 
plural behaviors from the respective behavior spaces to 
a common space, and computes a relative transforma- 
tion to a behavior-commanded object position in accor- 45 
dance with the object position perturbations of the plu- 
ral behaviors. A kinematics processor updates a trans- 
formation to a current commanded object position 
based upon the relative transformation to the behavior- 
commanded object position. A multiple arm squeeze 50 
control processor computes, from appropriate squeeze 
force input parameters and from actual squeeze forces 
for each of the arms, a squeeze control position pertur- 
bation for each of the arms, to provide squeeze control. 

An inverse kinematics processor computes, from the 55 
commanded object position transformation and from 
the squeeze control position perturbation, new robot 
joint angles, and controls respective joints of the robot 
arms in accordance with the new robot joint angles. 

The plurality of input parameters can include Carte- 60 
sian stiffness parameters, force setpoints, joint limits, 
joint singularities, dither wave parameters, teleopera- 
tion input frame of reference and a Cartesian trajectory. 
The plurality of behaviors can include Cartesian stiff- 
ness control, force control, joint limit avoidance, joint 65 
singularity avoidance, dither wave motion, teleopera- 
tion control and Cartesian trajectory control. The com- 
pliant motion processor computes the behavior-com- 
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manded object position by first computing the position 
perturbations for all of the plural behaviors and then 
combining all of the position perturbations. 

Where one of the behaviors includes Cartesian trajec- 
tory control, the system further includes a trajectory 
processor which computes a drive transformation to a 
relative object position in accordance with Cartesian 
trajectory input parameters, in which case the kinemat- 
ics processor updates the transformation to a current 
commanded object position based upon the relative 
transformation to the behavior-commanded object posi- 
tion and based upon the drive transformation each sam- 
pling interval. The drive transformation is computed in 
accordance with input parameters specifying initial and 
destination object positions and in accordance with the 
number of elapsed sampling intervals. 

Where one of the input parameters includes move 
force set points, the compliant motion processor com- 
putes a position perturbation of the object in accor- 
dance with a product of a force control constant and a 
difference between the move force set points and the 
actual move forces. Where the input parameters include 
joint angle limits and joint angle singularities for indi- 
vidual joints on the arms, the compliant motion proces- 
sor computes for each of the arms a position perturba- 
tion of the object in accordance with a product of a 
force field constant and a reciprocal of a difference 
between actual joint angles sensed by joint angle sensors 
on the arms and corresponding ones of the joint angle 
limits and joint angle singularities. 

The various input parameters such as joint angle 
limits, joint angle singularities and the force set points 
are comprised within a generalized compliant motion 
primitive of user-specified input parameters. The new 
robot joint angles are computed repetitively in succes- 
sive sampling intervals and the input parameters are 
changeable each sampling interval so that the plural 
behaviors are dynamically programmable. 

Where behaviors comprise safety monitoring or ter- 
mination condition monitoring, the system stops motion 
of the robot in response to predetermined quantities 
measured by sensors on the robot reaching certain val- 
ues specified by corresponding ones of the input param- 
eters. 

BRIEF DESCRIPTION OF THE DRAWINGS 

FIG. 1 is a diagram of the vectors employed herein 
pertaining to two robots holding a common object. 

FIG. 2 is a kinematic diagram illustrating the hierar- 
chy of coordinate transformations employed in carrying 
out the invention. 

FIG. 3 is a block flow diagram illustrating a control 
process of the invention. 

FIG. 4 is divided into two parts labelled FIG. 4A and 

FIG. 4B together comprising a schematic block dia- 
gram of a dual arm control system embodying the in- 
vention. 

FIGS. 5A and SB are graphs of force measurements 
over time and show forces, torques, and translation for 
an experiment in applying the invention to an autono- 
mous ORU insertion task. 

FIG. 6,is a graph of force measurements over time 
showing forces during shared control teleoperation 
ORU insertion. 

FIG. 7 is a graph of torque measurements over time 
showing torques during shared control teleoperation 
ORU insertion. 
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FIG. 8 is a graph of position measurements showing 
translations during shared control teleoperation ORU 
insertion. 

FIG. 9 is a graph of measured joint angle as a function 
of time showing joint limit avoidance in an experiment 5 
testing the joint limit avoidance feature of the invention. 

FIG. 10 is a graph of measured force over time show- 
ing forces during squeeze control exerted on the ORU. 

FIG. 11 is a graph of measured torque over time 
showing torques during squeeze control on the ORU. 10 
FIG. 12 is a graph of measured position over time 
showing translations during shared control fluid cou- 
pler mating task. 

FIG. 13 is a graph of measured forces and torques 
over time showing forces and torques during shared 15 
control fluid coupler mating task, 

FIG. 14 is a graph of measured torque over time 
showing torque during the autonomous fluid coupler 
turning task. 

FIG. 15 is a graph of measured force over time show- 20 
ing torque during the autonomous fluid coupler turning 
task. 

DETAILED DESCRIPTION OF THE 

PREFERRED EMBODIMENT 25 

I. Nomenclature 

The invention will be described in detail below with 
reference to the following definitions of nomenclature 
including nomenclature for transforms and coordinate 3 Q 
frames. 

Coordinate frames 

BASEL = base of left robot 35 

BASER = base of right robot 
DITHER=D=dither task frame 
FORCE=F=force control task frame 
GRL= point where left arm grasps object and left 
squeeze forces are controlled 40 

GRR= point where right arm gr 2 isps object and right 
squeeze forces are controlled 
MERGE=M= actual position of common object 
OBJDEST =a priori destination of common object 
OBJINIT =initial position of common object 45 

OBJPT =position of common object due to trajectory 
generator 

PTL= position of GRL after squeeze control perturba- 
tion 

PTR= position of GRR after squeeze control perturba- 50 
tion 

TELEOP =T= teleoperation task frame 
TNL= attached to terminal link of left robot 
TNR= attached to terminal link of right robot 
WORLD = common frame for specifying position of 55 
robot base frames and co mm on object position 


Transformations: 


trBaseL = 


WORLD ^ 
BASEL ^ 


trBaseR = 


WORLD ^ 
BASER 


trDither = 


MERGE 
DITHER ^ 


OBJDF^T 

trDrive = OJTPT ^ ~ trajectory generator motion 


trForce = 


MERGE ^ 
FORCE ^ 


trGrL — 


TNL rr 
GRL^ 


trGrR = 


TNR 

GRR 


T 


60 


65 
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■continued 


PTL 

trLObj = merge ^ ~ transform from left grasp point of 
object to object motion frame 


trObj = T = motion of the object from its initial position 

WORLD T* 
trObjDest — oBJDEST* 


trObjlnit — 


WORLD rj, 
OBJINIT 


trPtL = T = left ann squeeze control motion 
trPtObj = r = all sensor based motion 


ORR 

trPtR = T = right arm squeeze control motion 

trRObj = T = transform from right grasp point 

of object to object motion frame 

■ <-p i MERGE fp 

trTeleop = teleop ^ 


trTnL = 


BASEL rp 
TNL ^ 


trTnR = 


B/ISEJ? ™ 
TNR ^ 


Subscripts: 
a = actual 
d s= dither 
f = force 
j = joint 
p s= perturbation 
r = reference 
t = teleoperation 


II. Input Parameter Set 

The Dual-Arm Generalized Compliant Motion 
(DAGCM) primitive provides multi-sensor based dual- 
arm cooperative control task execution for a higher 
level planning system. The higher level system de- 
scribes the desired behavior of control via an input 
parameter set. The input parameter set is sufficiently 
general (as is the primitive) to specify a wide variety of 
supervisory and shared control tasks. The input param- 
eter set is given below. FIG. 2 illustrates the hierarch of 
coordinate transforms and coordinate frame definitions 
employed herein. 

System Parameters 

period; period to report status to higher level system 
trRObj: transform from right grasp frame to MERGE 
frame 

trBaseR; transform from WORLD to BASER frame 
trBaseL: transform from WORLD to BASEL frame 
massPropR: object mass properties felt by right arm 
massPropL: object mass properties felt by left arm 

Trajectory Parameters 

trTnDest: transform from BASER frame to destination 
right arm TNR frame 

timeSpeed: selects time or velocity based nominal mo- 
tion 

segVal: time or velocity to execute nominal motion 
cAcc: maximum Cartesian acceleration of nominal mo- 
tion 

Contact Force Control Parameters 

trForce; transform from MERGE frame to FORCE 
frame 

cfSelVect: hybrid selection vector for FORCE frame 
cfComplyVect: selection vector specifying which posi- 
tion DOFs of FORCE frame also have compliance 
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cfFtSetpoints: force setpoints in force controlled DOFs 
of FORCE frame 

cfFtGains: force gains in FORCE frame 
cfMaxFcVel: maximum velocities in DOFs of FORCE 
frame due to force control 

Teleoperation Control Parameters 

whichHC: which hard controller to use (right or left) 
teleMode: which teleoperation mode: tool, world, or 10 
camera 

trCamera: transform from WORLD frame to camera 
TN frame 

trTeleop: transform from MERGE frame to TELEOP 
frame 

tpSelVect: selection vector for shared control 
teleWeights: weightings for teleop inputs 
tpMaxVel: max velocity due to teleoperation inputs 

Dither Control Parameters 

trDither: transform from MERGE frame to DITHER 
frame 

dtWaveMag: magnitudes of dither functions 
dtPeriod: periods of dither functions 25 


Termination Condition Monitor Parameters 

select; bit mask to select which ending conditions to test 
for 

testTime: time over which to average ending condition 
variables 

endTime; maximum ending motion time 
endTransDel: total translation due to sensor based mo- 
tion in MERGE frame 

endAngDel: total angular motion due to sensor based 
motion in MERGE frame 

endTransVel: magnitude of rate of change of endTrans- 
Del 

endAngVel: magnitude of rate of change of endAngDel 
endForceErr: contact force error vector magnitude 
endTorqueErr: contact torque error vector magnitude 
20 endForceVel: magnitude of rate of change of endFor- 
ceErr 

endTorqueVel: magnitude of rate of change of endTor- 
queErr 

III. Move-Squeeze Decomposition 


Joint Limit and Joint Singularity Avoidance 

jaGain: gain for joint limit and singularity avoidance 
JaThres: threshold from joint limit or singularity 

Stiffness Control Parameters 

spSelVect: degrees of freedom in which to apply 
springs 

spGains: spring gains 
spMaxVel: max velocity due to springs 

Squeeze Force Control Parameters (specified sepa- 
rately for both right and left arms) 

trGrL, trGrR: transform from TN frame to GR frame 
sqForceSpL, sqForceSpR: squeeze forces set points in 
GRR and GRL frames 
sqFtGainsL, sqFtGainsR: force control gains for each 45 
axis of squeeze control 

sqMaxVelL, sqMaxVelR: max velocity due to squeeze 
force control 

sqSpGainsL, sqSpGainsR: squeeze spring gains 

Control Monitor Parameters 

ctFThres: contact force threshold 
ctTThres: contact torque threshold 
sqRFThres: right squeeze force threshold 
sqRTThres: right squeeze torque threshold 
sqLFThres: left squeeze force threshold 
sqLTThres: left squeeze torque threshold 

Fusion Monitor Parameters 

fsPThres: position threshold for sensor based motion 
fsOThres: orientation threshold for sensor based motion 


Multiple arms holding an object can apply both 
forces which cause motion of the object and forces 
which cause internal forces to build up within the object 
30 without motion. The former are called external or move 
forces and the later are called internal or squeeze forces 
[4,5]. Throughout this specification, “position” will 
refer to both location and orientation, and “force” will 
refer to both force and moment. Execution of a dual- 
arm task requires the control of both of these force 
subspaces. Description of the desired contact interac- 
tion between the commonly held object and its environ- 
ment requires description of the desired move subspace 
4 Q forces. Description of the desired internal object forces 
requires description of the squeeze subspace forces. 

Decomposition of the forces applied by multiple ro- 
bots on an object into move and squeeze subspace com- 
ponents was originally described in [4] and subsequently 
by various authors [5-10]. An earlier method for coop- 
erative motion of two manipulators was “follow the 
leader” where the motion of the leader manipulator was 
planned and the motion of the follower manipulator 
was determined based upon the motion of the leader 
[11,12]. The move-squeeze decomposition used here 
follows the method described in [4]. It is assumed that 
the arms and the commonly held object are rigid and 
the grasps between the arms and object are rigid. 

55 Two robots holding a common object are depicted in 
FIG. 1. One robot grasps the object at location Gi and 
a second robot grasps the object at location Gz. A third 
point on the object, C, is the location of potential 
^ contact with the environment. The two arms exert 
forces and torques on the object, Fi, Ni and F 2 , N 2 , 
which are combined in the vectors fi and fz, i.e.. 


Joint Monitor Parameters 65 

jSafetyLim: joint safety limits from singularities and 
joint limits 



The vector of forces at C is then 
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/c = *c,l -/l + <)>c,2-/2 

= ^^-/l2 
where 

A'T = $<,, 2 ] 

-G] 



The vectors ri and 12 in FIG. 2 are 


10 

ring equations are used to describe the motion of the 

( 1 ) common object, the two arms and the relationships 
between them. The kinematic ring equations for the 
common object, right arm, and left arm are 

5 

trObjInit.trObj.trPtObj=trObjDest.trDrive (10) 

trBaseL.trTnL.trGrL.trPtL.trLObj = trObjInit. 
trObj (11) 

(2) 10 trBaseR.trTnR.trGrR.trPtR.trRObj=trObjInit. 

( 3 ^ trObj (12) 

The motion of the object is computed independently 
of the motion of the manipulators, except to compute 
the initial position. The entire process is illustrated in 
FIG. 3. In Equation 10, trObjInit and trObjDest are 
constant transforms which must be computed or pro- 
vided initially, as in block 10 of FIG. 3. trObjInit is 

® 20 known initial position of the right 

^ robot, trTnR;nir, and the input parameters trBaseR, 
trGrR and trRObj. 

trObjInit = trBaseR*trTnR/n/rtrGrR*trRObJ (13) 

trObjDest is computed from input parameters. 



The components of the force vector /12 which are in 
the nullspace of comprise the squeeze forces [4], / 12 s, 
and the components of /12 which are in the vector space 
of comprise the move forces, /i 2 m> i-e., 35 

fl2=/l2j+/l2m (6) 

The move forces are computed using the Moore-Pen- 
rose inverse of A^, i.e., 40 

( 7 ) 

The force vector fn is known by measurement with 
wrist force-torque sensors. The squeeze forces are then 45 
computed with 

fl2s=/l2-fl2m (8) 

The move and squeeze forces (and torques) for the two 
manipulators are then extracted from / 12 m and fizs 

/l2m=[Nlm.Flm.N2m.F2m]^ 

/l2i=[Nu,Fli,N2„F2,F (9) 

The actual move and squeeze forces are then avail- 55 
able for use in control. The mass properties of the com- 
mon object, as felt by each arm, are given in the mas- 
sPropR and massPropL input parameters. The gravity 
forces of the composite object beyond the force sensors 
are computed and subtracted from the measured /12 60 
before move-squeeze decomposition is computed. 

rV. Kinematic Relationships And Nominal Motion 


trObjDest=trBaseR.trTnDest.trGrR-trRObj (14) 

Alternatively, trObjInit and trObjDest could have been 
given as input parameters. 

The transforms trObj, trPtObj, trPtL and trPtR are 
all initially the identity transform, as in block 12 of FIG. 
3. They are then updated each sample interval. In block 
13 of FIG. 3, move-squeeze decomposition described in 
Section 3 hereof is used to compute the actual squeeze 
forces (as well as the actual move forces). The updating 
of trPtObj corresponds to block 14 of FIG. 3. 

The trDrive transform generates the reference trajec- 
tory and is interpolated, as in block 16 of FIG. 3, from 
an initial value which solves Equation 10 with trObj and 
trPtObj both the identity transform to a final value, 
namely the identity transform. The Cartesian interpola- 
tion is described by the input parameters timeSpeed, 
segVal and cAcc. Initially, the trDrive transform is the 
transformation between the initial and destination ob- 
ject positions, and incrementally approaches the iden- 
tity transform as the object approaches the destination 
point each sampling interval. Each incremental change 
is thus interpolated based upon the foregoing input 
parameters. 

The trPtObj transform is updated each sample inter- 
val in block 14 of FIG. 3 with the integration of all 
sensor based motion as described in Section VI below 
with reference to FIG. 4. After trDrive and trPtObj are 
computed, the trObj transform is computed using Equa- 
tion 10, as in block 18 of FIG. 3. trObj is then used in the 
left and right robot ring equations to compute their new 
positions, corresponding to the steps of blocks 22 
through 26 of FIG. 3 for the left arm, as will now be 
described. 

The motion of the left and right robots are treated 
equally, each dependent on the motion of the common 


The kinematic relationships in DAGCM between the object and the input parameters. In Equation 11, the 
two manipulators, the common object, and the various 65 trBaseL, trGrL and trLObj transforms are constant and 
motion sources are shown in FIG. 2 , showing the vari- given in the input parameters. Each sample interval the 
ous coordinate frames and the transformations between trPtL transform is computed by squeeze control as 

them (defined in the Nomenclature above). Kinematic described in Section V below, and then trTnL is com- 
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puted using Equation 11. Computations for the right 
robot are equiv^ent to the left except for variations due 
to different input parameters. Once trTnL and trTnR 
are computed each sample interval, they are used in 
inverse kinematics for each arm to compute the equiva- 
lent joint angles for the robots. Joint position servos 
then drive the joints to the new computed angles. 

V. Squeeze Control 

Squeeze control is used to compute trPtR and trPtL 
of Equations 1 1 and 12. This portion of the process is 
illustrated in FIG. 3 as two parallel branches involving 
identical steps, one for the right arm and one for the left 
arm. Both branches will be described principally with 
reference to the left arm. In each sample interval move- 
squeeze decomposition, as explained in Section III, is 
used to compute the squeeze forces (as well as move 
forces) in the GRL and GRR coordinate frames begin- 
ning with the step of block 13 of FIG. 3. Either or both 
of the robots can be controlled to track the desired 
squeeze force with the choice specified by the input 
parameter set. Squeeze control will be described for 
only the left arm here although it is equivalent for the 
right arm. In the step of block 22 of FIG. 3, at each 
sample interval the vector of six perturbations along 
and about the axes of the GRL frame is computed with 


12 

tory generator, force control of the move forces from 
move-squeeze decomposition, and motion based upon 
several other sensors. As used herein, the term “sensor” 
is very general and refers to such motion control 
5 sources as telerobotic user hand controls, as one exam- 
ple. Generalized compliant motion is a control algo- 
rithm which allows multiple sensor based control for 
task execution [13,14]. The sensors can be either real, 
e.g., a force-torque sensor, or virtual, e.g., a computed 
distance to collision. Each sensor is provided an individ- 
ual task space for control and the resulting motion com- 
manded by each sensor is merged in a common frame 
MERGE. 

A block diagram for DAGCM is given in FIG. 4. 
Each sensor generates a desired perturbation for the 
object in the sensor frame’s individual task frame. This 
perturbation is then transformed to an equivalent per- 
turbation of the common MERGE frame assuming the 
sensor task frame and MERGE frame are connected by 
a rigid body. The commanded perturbation of the 
MERGE frame for a sample interval due to all sensor 
based motion is then 

25 ( 17 ) 


BXlsq — Xjs^(fisgr — ( 15 ) 

where is the diagonal matrix of gains given in the 
input vector sqFtGainsL, fugr are the desired (refer- 
ence) squeeze forces given in the sqForceSpL vector of 
input parameters and fuqa are the measured actual 
squeeze forces computed in move-squeeze decomposi- 
tion of the previous step of block 13 of FIG. 3. The 
sqMaxVel vector input parameter is then used to limit 
the magnitude of the pertubations in the vector dXtaj- In 
the next step of block 24 of FIG. 3, the trPtL transform 
is updated with the new perturbations with 

trPtL=trDelisq-trPtL ( 16 ) 40 

where trDelfa^ is generated from SXisg as explained in 
the following paragraph. 

Given a vector of small perturbations, 8X, where 

45 

S.y=[<it<i)’.<fe8a,dj8,6y]r 

a perturbation transform can be generated with 

IrZ>e/8X’=trans(.x,<£ic).transO,d)’)-trans(s</z).rot(ji8a)- ,« 

-rot(j>,8/S).rot(2,S'y) ^ 

where trans(v,d) is a translation of d along the v axis and 
rot('U,8) is a rotation of 8 about the v axis. The order of 
transform multiplication does not matter since the per- 
turbations are assumed small. 

In the step of block 26 of FIG. 3, the system computes 
trTnL from trPtL using Equation 11. In the step of 
block 28 of FIG. 3, inverse kinematics are employed to 
compute new joint angles from the updated trTnL 
transform. A parallel sequence of steps is performed for 
the right arm, as illustrated in FIG. 3. Both robot arm 
joints are then moved accordingly. Then, the system 
goes to the next sampling interval at block 30 of FIG. 3 
and the entire process is restarted at the step of block 14. 

65 

VI. Generalized Comphant Motion 

Generalized compliant motion is used here to control 
the motion of the common object based upon the trajec- 


The Jacobians used in Equation 17 relate Cartesian 
perturbations at two different frames attached to the 
same rigid body, e.g., transforms perturbations in 
the F frame to equivalent perturbations in the M frame. 
Throughout this specification, a preceding subscript 
indicates the frame that the quantity is transformed 
from and a preceding superscript indicates the frame 
that the quantity is transformed to. 

The first term in Equation 17 is the stiffness term. 
The matrix of gains is a diagonal matrix of the input 
vector parameters spGains and Xp represents the accu- 
mulated motion due to sensor based control which is 
stored in trPtObj. Additionally, the spSelVect input 
vector selects which degrees of freedom in the 
MERGE frame to apply stiffness and the spMaxVel 
input vector specifies the maximum velocity for motion 
due to stiffness. Equation 17 represents the computa- 
tions in matrix/vector form for the six degrees of 
freedom. However, in implementation for a single arm 
robot the three orientation degrees of freedom were 
treated as one rotation about an equivalent axis of 
rotation [14]. The perturbation transform due to stiff- 
ness, ■*^trDels, is then computed from the stiffness per- 
turbations. 

The second term in Equation 17 is the force control 
term. The matrix of gains K/ is a diagonal matrix of the 
input parameters cfFtGains and /;-and /a are the desired 
(reference) and actual move forces measured in the 
FORCE frame, fr are given in the cfFtSetPoints input 
vector. The fa forces are the move forces computed 
using move-squeeze decomposition with Equation 7. 
The degrees of freedom for force control are selected 
with the cfSelVect and cfComplyVect input vectors. 
The magnitude of the computed perturbation due to 
force control in the FORCE frame is then limited as 
specified by the cfMaxFcVel input vector. The vector 
of perturbations is then transformed to the MERGE 
frame and the perturbation transform due to force con- 
trol, ■^trDel/, is then computed from the perturbations. 

The T^J-^SXf term of Equation 17 represents the 
motion due to teleoperation inputs. The motion begins 
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as motion of a six DOF hand controller in the operator 
control station. The joint perturbations each sample 
interval are converted to equivalent Cartesian perturba- 
tions using the hand controller Jacobian. The perturba- 
tions are then mapped to the equivalent perturbations of 5 
the common object in the TELEOP frame, depending 
upon the various input parameters. Explanation of the 
mappings of teleoperation inputs is beyond the scope of 
this specification. The result are the teleoperation per- 
turbations ^SXf which are then transformed to the lo 
equivalent perturbations in the MERGE frame using 
7 * 9 . The perturbation transform due to teleoperation, 
^'trDel;, is then computed from the perturbations. 

The zj*9-®6X£f term in Equation 17 represents motion 
due to dither signals. Triangular dither waveforms are 15 
generated in each DOF of the DITHER frame as speci- 
fied by the dtWaveMag and dtPeriod input vectors. 
These generate the ^ 6 X</ dither perturbations each sam- 
ple interval which are then transformed to the MERGE 
frame. The perturbation transform due to dither, 20 
*^trDeld, is then computed from the perturba- 
tions. 

Joint space joint limit and joint singularity avoidance 
is represented by the *^SX^- term of Equation 17. As a 
joint limit or joint singularity is approached, a joint 25 
perturbation is computed, 80i, based upon a repelling 
force. 

kei (18) 

^actuali — ^Umiti 30 


14 

forces based upon current robot force sensor outputs 
(block 13 of FIG. 3). A compliant motion processor 140 
of FIG. 4 transforms pluraJ object position perturba- 
tions of the plural behaviors from the respective behav- 
ior spaces to a common space, and computes a relative 
transformation to a behavior-commanded object posi- 
tion from the object position perturbations of the plural 
behaviors (block 14 of FIG. 3). A trajectory processor 
160 of FIG. 4 computes, from the transformations to (a) 
the object initial position and (b) the object destination 
position, and from the current time, a drive transforma- 
tion to a relative object position (block 16 of FIG. 4). A 
kinematic processor 180 of FIG. 4 updates a transforma- 
tion to a current commanded object position based upon 
the relative transformation to the behavior-commanded 
object position and based upon the drive transformation 
each sampling interval (block 18 of FIG. 3). A multiple 
arm squeeze control processor 220 of FIG. 4 computes, 
from appropriate squeeze force input parameters and 
from actual squeeze forces for each of the arms, a 
squeeze control position perturbation for each of the 
arms to provide squeeze control (blocks 22 and 24 of 
FIG. 3). An inverse kinematics processor 280 of FIG. 4 
computes, from the commanded object position trans- 
formation and from the squeeze control position pertur- 
bation, new robot joint angles (blocks 26 and 28 of FIG. 
3). The robot servos then move corresponding joints of 
the arms to the new robot joint angles. 

VII. Safety Monitoring 


where ka, is the gain for joint limit i or joint singularity 
i. Qactuali is the actual joint angle, and dumiti is the limit 
that the joint is approaching, either as a joint limit or 
singularity. k« is element i in the jaGain input vector. 35 
The joint limit and joint singularity perturbation is only 
computed if the distance Oactualt-^Umiti is less than the 
threshold given in the jaThres input vector. Joint limit 
and joint singularity avoidance is computed for both 
robots. 40 

The perturbations for a sample interval in the 
MERGE frame are 

( 19 ) 45 

where 80;j and ddijaie the vectors of joint space pertur- 
bations for the right and left arms. There are two types 
of Jacobians used in Equation 19, Jacobians relating 
Cartesian perturbations at two different frames attached 
to the same rigid body, e.g., and Jacobians ^ 

which relate joint space perturbations to Cartesian per- 
turbations, e.g., ™^3tnr- The perturbation transform 
due to joint space joint limit and joint singularity avoid- 
ance, -VtrDely, is then computed from the ^dXj pertur- 
bations. 

The sensor based motion transform, trPtObj, is up- 
dated each sample interval using all of the sensor pertur- 
bation transforms. 

^trPtObJ=^trDels'^trDeIf-^trDelr^trDeIj-^t- 

rDelj-^ttrPtObj (20) 

Equation 20 is the actual implementation of Equation 
17. 

In summary, the operation of the system of FIG. 4 65 
may be described with reference to FI(j. 3 as follows: A 
move-squeeze decomposition processor 130 of FIG. 4 
computes actual move and squeeze decomposition 


Safety is a primary concern during task execution. 
The DAGCM primitive provides monitoring of various 
states of the system. Several force and torque thresholds 
are monitored. If the magnitude of the force or torque 
vector exceeds the input threshold, the motion is 
stopped and the cause is sent back to the calling system. 
ctFThres and ctTThres are the thresholds for monitor- 
ing the contact (move) forces and sqRFThres, 
sqRTThres, sqLFThres and sqLTThres are the thresh- 
olds for monitoring the squeeze forces. The accumu- 
lated motion due to sensor based control, which is rep- 
resented by trPtObj, is also monitored. The fsPThres 
and fsOThres thresholds are the maximum allowable 
translational and angular motion due to sensor based 
motion. The jSafetyLim threshold is the minimum al- 
lowable distance to a joint limit or singularity. 

VIII. Termination (Condition Monitoring 

The termination condition monitor tests for satisfac- 
tion of input termination conditions and stops motion if 
the input conditions are satisfied. Termination condi- 
tions are not tested until the trajectory generator has 
completed (the trDrive transform is identity). All sensor 
based motion continues until the selected termination 
conditions are satisfied, or until endTime seconds have 
past since the trajectory generator finished. The condi- 
tion for termination of motion is sent back to the calling 
system. 

IX. Shared Control 

Shared control is the merging of teleoperation and 
autonomous control in real time during task execution. 
This may take various forms. Compliant teleoperation is 
where the operator controls the motion of the object 
and the autonomous system controls the contact (move) 
and squeeze forces. Partitioned shared control is where 
certain task space degrees of freedom are controlled by 
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the operator with a hand controller and the others are 
controlled by the autonomous system. The task primi- 
tive described in this specification provides shared con- 
trol as specified by the input parameters. For parti- 
tioned shared control, the tpSelVect input vector se- 5 
lects which degrees of freedom are to be controlled by 
the operator via the hand controller. 

X. Implementation Environment 

The DAGCM task execution primitive has been im- 10 
plemented in the JPL Supervisory Telerobotics 
(STELER) laboratory. The primitive controls two 
PUMA 560 task execution manipulators equipped with 
LORD wrist force-torque sensors and parallel jaw ser- 
voed grippers. The implementation environment is de- 15 
scribed in more detail in [2]. The Cartesian level dual- 
arm control was run with a 10 ms sample rate. Joint 
position servo control was run with a 1 ms sample rate. 

XI. Experimental Results 20 

The task primitive has been utilized for execution of 
various cooperative dual-arm space telerobotic tasks 
both under supervisory and shared control. The tasks 
are described below along with the primitive parame- 
ters used for the tasks. Experimental results of some of 25 
the tasks are also given. The tasks described below all 
have trTeleop and trForce set to the identity transform. 
Therefore the MERGE, FORCE, and TELEOP 
frames are the same. In the discussion, contact forces 
and torques are the forces and torques in the move 30 
space. Contact and move space are used interchange- 
ably. 

XI. 1 Orbital Replacement Unit Manipulation 

The dual-arm orbital replacement unit (ORU) 35 
changeout task was accomplished using both supervi- 
sory and shared control. The ORU has two pins, each of 
which extends from the bottom of the ORU directly 
beneath a grapple lug. The ORU is grasped by both 
nmnipulators at the stowbin (tilted in the background of 40 
the photograph), the ORU is removed, moved to an 
approach location above the platform (between the two 
manipulators), and inserted into the platform. The pa- 
rameters common to all of the ORU tasks are given 
below. The mass properties, massPropR and mas- 45 
sPropL, of the load as seen by the two manipulators 
were the same so that the load was shared equally. The 
mass was 3.8 N. The vector, with respect to the TN 
frame, to the center of mass (in mm) was (120, 0, 250) 
for the left arm and (— 120, 0, 250) for the right arm. 50 
The transform trRObj was a translation of —330 mm 
along the X axis. trForce was the identity transform. 
cfSelVect was (1,1, 1,1, 1,1) and cfComplyVect was 
(0,0,0,0,0,0) so that all six DOFs were force controlled. 
The contact force control gains, cfFtGains, were (0.02, 55 
0.02, 0.02, 0.00002, 0.00002, 0.00002) where transla- 
tional gain units are mm/N and orientational gain units 
are deg/N-mm. whichHC was set to use the right hand 
controller. teleMode was set to camera mode although 
tool and world modes were also available. trCamera 60 
was automatically set based upon the current position of 
the camera robot. trTeleop was set to the identity trans- 
form. tpSelVect was set to (1,1, 1,1, 1,1) so that the oper- 
ator could control all six DOF if desired. teleWeights 
was set so that there was approximately a one to one 65 
mapping from hand controller motion to manipulator 
motion. Dither and stiffness control were not used in 
the experiments. trGrR and trGrL were both transla- 


tions of 400 mm along the Z axis which corresponded to 
grasp frames 130 mm below the actual grapple lugs. 
Squeeze control force control gains, sqFtGainsL and 
sqFtGainsR, were (0.04, 0.04, 0.04, 0.00004, 0.00004, 
0.00004) where translational gain units are mm/N and 
orientational gain units are deg/N-mm. Squeeze control 
spring gains, sqSpGainsL and sqSpGainsR, were all set 
to 0. ctFThres and ctTThres were 200N and 45000 
N-ram, respectively. sqRFThres and sqRTThres were 
200N and 45000 N-ram, respectively. sqLFThres and 
sqLTThres were set equal to sqRFThres and 
sqRTThres. fsPThres and fsOThres were set large so 
that they would not be tripped. jSafetyLim was set to 10 
deg. Termination conditions were not used in the exper- 
iments to stop the motion. Motion was stopped on oper- 
ator interrupt for teleoperation and shared control, or 
time or position threshold (fsPThres) for autonomous 
control. All sensor based motion maximum velocities 
were set to large-values for the experiments so that the 
control would not be affected by velocity limiting. 

The autonomous control sequence to remove, trans- 
late, and insert the ORU will now be described. Joint 
limiting was not used for autonomous control since 
autonomous control tasks should be planned a priori 
and joint limits should not be encountered. Also, joint 
limit control during contact tasks could cause joint 
space motions which would cause undesired Cartesian 
space motions. The squeeze force setpoints, sqFor- 
ceSpL and sqForceSpR, were set to 0 so that no inter- 
nal forces would build up in the ORU. The autonomous 
remove and insert tasks relied on contact force control 
(move space) to cause motion so trTnDest was automat- 
ically set to cause no trajectory generator based motion, 
timeSpeed was set to time, and segVal to a time longer 
than the remove or insert tasks should take, 8 sec. For 
the dual-arm ORU removal task, the force control set- 
points, cfFtSetpoints, were set to zero except for the 
setpoint along the Z axis which was set to 25N. The 
fsPThres threshold was set to 150 mm. Control of the Z 
axis contact force to 25N caused the manipulators to 
pull the ORU out of the stowbin. When the ORU 
moved 150 mm under sensor based motion force control 
in this case), then the fusion monitor signaled that the 
fsPThres was exceeded and the motion stopped and the 
cause of termination was returned to the operator con- 
trol station and displayed. 

For autonomous translation from the stowbin to the 
approach location above the platform, the contact force 
gains, cfFtGains, were set to zero, as were the squeeze 
control gains, sqFtGainsR, for the right arm. The 
squeeze control gains for the left arm were the same as 
for the remove task. This way the right and left arms 
would follow the nominal trajectory of the common 
object and any internal forces that built up were dissi- 
pated by squeeze control with the left arm. The ORU 
would go to the proper destination since the right arm 
moved only kinematically - with no sensor based mo- 
tion. Two guarded motion commands were used: first 
to a via point, and then to the platform approach posi- 
tion. As indicated in FIG. 6A, the approach position 
was approximately 90 mm above the inserted position. 
trTnDest was used to specify the destinations and 
timeSpeed was set to velocity based motion. 

The parameters for the autonomous ORU insert task 
were the same as for the autonomous remove task ex- 
cept for the force to cause insertion and the distance 
threshold. For the dual-arm ORU insert task, the force 
control setpoints, cfFtSetpoints, were set to zero except 
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for the setpoint along the Z axis which was set to 
— ION. The negative value caused the manipulators to 
push the ORU into the platform so that the pins inserted 
into the passive connector holes. The fsFIlires thresh- 
old was set to a distance greater than expected for the 5 
insertion (e.g., 200 mm) so that the motion would stop 
on the endTime termination condition of 0 sec. after the 
8 sec. task execution time specified with segVal. This 
was used rather than the expected insertion distance 
because the insertion distance could not be known ex- 10 
actly and it was desired to insert as far as possible. Other 
termination conditions might also be used such as end- 
TransVel, endAngVel, endForccErr, endTorqueErr, 
endForceVel, and endTorqueVel. 

FIGS. 5A and SB show forces, torques, and transla- 15 
tion for the experiment. The right and left arm squeeze 
force vector magnitudes were equal within 0.005 IN. In 
FIG. 5A showing an autonomous ORU insertion task, 
the solid line is the contact force along FORCE Z, 
while the dashed line is the translation along MERGE 20 
Z. In FIG. SB showing an autonomous ORU insertion 
task, the line with no symbol is the contact torque vec- 
tor magnitude in FORCE, indicates the left arm 
squeeze torque vector magnitude in GRL, □ indicates 
the right arm squeeze torque vector magnitude in GRR, 25 
A indicates the combined magnitude of X and Y compo- 
nents of the contact force vector, and indicates the 
left arm squeeze force vector magnitude in GRL and 
right arm squeeze force vector magnitude in GRR. 
FIG. 5A shows that force control caused motion along 3C 
the Z direction until the ORU was inserted (approxi- 
mately 90 mm); then the force increased to its setpoint 
since motion was now constrained in this DOF (there 
seems to be an approximately 2N bias in the Z force, 
perhaps due to an incorrect mass property). Free mo- 35 
tion to contact between the ORU pins and the chamfers 
leading into the holes occurs during the first 0.5 sec. 
One or both of the pins then slide down the chamfer 
until approximately the 1.5 seconds point when the pins 
then are in the hole and there is little resistance to the 4( 
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during shared control teleoperation ORU insertion, the 
indicates contact force vector magnitude in FORCE, 
and □ indicates the left arm squeeze force vector magni- 
tude in GRL and right arm squeeze force vector magni- 
tude in GRR. In FIG. 7 showing torques during shared 
control teleoperation ORU insertion, the indicates 
contact torque vector magnitude in FORCE, □ indi- 
cates left arm squeeze torque vector magnitude in GRL, 
and A indicates right arm squeeze torque vector magni- 
tude in GRR. In FIG. 9 showing translations during 
shared control teleoperation ORU insertion, the solid 
lines are actual sensor based motion in MERGE, dashed 
lines are integrated teleoperation inputs in TELEOP, 
indicates X, □ indicates Y, and A indicates Z. 

The integrated teleoperation inputs of FIG. 9 are the 
sum of the teleoperation perturbations per sample inter- 
val. The actual motions along the different axes of the 
MERGE frame are less than the integrated teleopera- 
tion inputs because of force control. Force control 
caused motion to reduce the contact forces and torques. 

Joint limit avoidance was demonstrated for free space 
teleoperation of the ORU. The parameters for shared 
control teleoperation ORU insertion above were used 
for the joint limit avoidance experiment except the gain, 
JaGain, was set to 1.64 deg2 and JaThres was set to 10 
deg. Joint limit avoidance worked well except for one 
case: when joint 5 of both manipulators approached 
joint limits at the same time. Then the joint limiting 
control of the two arms counteracted each other and 
rather than moving away from the limits, the limits 
were exceeded. For all joints and for all other cases, the 
joint limiting control worked properly to avoid joint 
limits. In the joint limit avoidance experiment of FIG. 9, 
the operator input rotation about the TELEOP frame Y 
axis which caused joint 5 of the right arm to approach 
its limit. FIG. 9 shows the results of the joint limit 
avoidance experiment. In FIG. 9 showing joint limit 
avoidance, the dashed line is the integrated teleopera- 
tion input about TELEOP Y, solid line is the actual 
rotation about MERGE Y and the dotted line is the 


insertion imtil the insertion is complete. The spikes in distance to the joint 5 Umit. FIG. 9 shows that when the 

the forces are due to collisions between one of the pins Cartesian teleoperation inputs woxild have caused joint 

and a ledge inside its hole (this internal ledge should 5 to move within its joint margin (parameter jaThres 

have been tappered to eliminate this). There is corre- = 10 deg), joint limit avoidance caused Cartesian mo- 

spondence between the magnitude of the force and the 45 tion to keep the joint out of its joint margin, 
slope of the position curve. The motion step each sam- Two tasks were conducted to demonstrate squeeze 
pie interval is proportional to the error in force. There- control In the first task, the ORU was squeezed (actu- 

fore, when there is more resistance to motion, i.e., ally pulled apart) with a force of 30N during teleopera- 

contact with a surface, then the force will increase and tion in free space. The default parameters above were 

the rate of motion will decrease. FIG. 5B shows that the 50 used except the squeeze force setpoints, sqForceSpL 
forces and torques in the other DOFs remained small and sqForceSpR, along the X axes were set to — 30N 

during the task, as desired. for the right arm and 30N for the left arm. FIGS. 10 and 

Execution of the remove, translate, and insert task 11 show forces and torques from the experiment. In 

using shared control teleoperation required only one FIG. 10 showing forces during squeeze control on 

command which initiated the task. Joint limiting was 55 ORU, □ indicates left arm squeeze force along GRL X 
not used since contact tasks were involved. No trajec- axis, A indicates combined magnitude of GRL Y and Z 

tory generator motion was used so trTnDest was auto- components of the left arm squeeze force vector and 

matically set to cause no trajectory generator based indicates move space force vector magnitude in the 

motion, timeSpeed was set to time, and segVal was set FORCE frame. In FIG. 11 showing torques during 

to a time longer than the task should take. The contact 60 squeeze control on ORU, □ indicates the move space 
force control setpoints, cfFtSetpoints, were set to zero torque vector magnitude in the FORCE frame, indi- 

and gains set to the default values above. The squeeze cates the right arm squeeze torque vector magnitude in 

control setpoints and gains were set to the default val- GRR, and A indicates the left arm squeeze torque vec- 

ues above. FIGS. 7, 8 and 9 show forces, torques, and tor magnitude in GRL. FIGS. 10 and 11 show that an 

translations from the shared control teleoperation ORU 65 internal squeeze force was generated in about 1 second 
insertion task. The right and left arm squeeze force and and then maintained while the forces and torques in the 

torque vector magnitudes were equal within 0.0083N other DOFs of the squeeze space and in all DOFS of the 

and 0.033 N-m, respectively. In FIG. 6, showing forces move space remained small. 
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In the second task demonstrating squeeze control, an 
object with two sections held together with rubber 
bands was squeezed (actually pulled apart as in the 
ORU experiment) so that the sections separated. The 
object was connected with eight rubber bands (four on 
top and four on the bottom). Each manipulator was 
given the mass properties of one half of the object. The 
right arm load mass properties were then 2.9N and 
center of mass (—27, 4.5, 241) in mm. The left arm load 
mess properties were 2.8N and center of mass (23,— 5.6, 
243) in mm. The other parameters were the same as in 
the squeeze control task for the ORU above. The 
DAGCM primitive was used for teleoperation of the 
object while simultaneously maintaining a constant 
squeeze force of 30 Newtons to maintain a separation of 
the two halves of the object (of about 4 cm). 

XI.2 Fluid Coupler Manipulation 

A combination of shared control teleoperation and 
supervised autonomy was used for the fluid coupler 
mating and turning task. 

The parameters common to all of the fluid coupler 
tasks were the same as the default parameters above for 
the ORU tasks except for those given below. The mass 
properties, is massPropR and massPropL, of the load us 
seen by the two manipulators were the same so that the 
load was shared equally. The mass was 2.8N. The vec- 
tor, with respect to the TN frame, to the center of mass 
(in mm ) was (50, 4, 235) for the left arm and (—50, 4, 
235) for the right arm. The transform trRObj was a 
translation of —140 mm along the X axis. trGrR and 
trGrL were both translations of 250 nun along the Z 
axis which corresponded to grasp frames approximately 
60 mm below the actual grasp locations. Squeeze con- 
trol force control gains, sqFtGainsL and sqFtGainsR, 
were (0.01, 0.01, 0.01, 0.00001, 0.00001, 0.00001) where 
translational gain units are mm/N and orientational gain 
units are deg/N-mm. The first task was shared control 
teleoperation of the fluid coupler to the satellite and 
approximate alignment of the coupler for insertion. 

The second task was shared control insertion of the 
coupler. The operator moved the coupler with teleop- 
eration inputs in all six DOF and a force setpoint along 
the FORCE frame Z axis of — 20N caused the autono- 
mous system to push the coupler in. The tight toler- 
ances between the mating parts and the required accu- 
rate alignment made the task difficult. The autonomous 
force control aided the operator by pushing the coupler 
in while the operator aligned it. Also, the autonomous 
force control kept the coupler in place once it was 
properly seated. FIGS. 12 and 13 show translations, 
forces, and torques from the experiment. The right and 
left arm squeeze force and torque vector magnitudes 
were equal within 0.0063N and 0.077N-m, respectively. 
In FIG. 12 showing translations during shared control 
fluid coupler mating task, the solid lines are actual sen- 
sor based motion in MERGE, the dashed lines are inte- 
grated teleoperation inputs in TELEOP, indicates X, □ 
indicates Y, and A indicates Z. In FIG. 13 showing 
forces and torques during shared control fluid coupler 
mating task, indicates the move space torque vector 
magnitude in FORCE, □ indicates the left arm squeeze 
torque vector magnitude in GRL, the line with no sym- 
bol is contact force along FORCE frame Z axis, A 
indicates the combined magnitude of FORCE frame X 
and Y components of tile contact force vector, and 
indicates the left arm squeeze force vector magnitude in 
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GRL and right arm squeeze force vector magnitude in 
GRR. 

The differences in FIG. 15 between the actual motion 
in the different DOFs and the motion specified by the 
5 teleoperation inputs was due to force control. The te- 
leoperation inputs were used to align the coupler. FIG. 
13 shows that the force along FORCE Z was controlled 
to — 20N while forces and torques in the other DOFs 
were dissipated. The differences from the setpoints 
10 were due to the impacts caused by the teleoperation 
inputs. 

The third task was turning and locking the fluid cou- 
pler. This was accomplished by controlling the squeeze 
forces to be zero and the contact (move space) forces to 
15 have a force pushing the coupler into the nozzle 
(— 20N) and a torque to cause the turning (— 8N-m). 
The cfFtSetpoints input vector was therefore (0,0,— 20 
N,0,0,— 8N-m) FIGS. 14 and 15 show rotation, torques, 
and forces for the experiment. The right and left arm 
20 squeeze force and torque vector magnitudes were equal 
within 0.0037N and 0.096N-m, respectively. In FIG. 14 
Showing the autonomous fluid coupler turning task, the 
solid line is the contact torque about FORCE Z, and the 
dashed line is the rotation about MERGE Z. In FIG. 15 
25 showing the autonomous fluid coupler turning task, the 
line with no symbol is contact force along FORCE 
frame Z axis, A indicates the combined magnitude of 
FORCE frame X and Y components of the contact 
force vector, indicates the left arm squeeze force 
30 vector magnitude in GRL and right arm squeeze force 
vector magnitude in GRR, indicates the combined 
magnitude of FORCE frame X and Y components of 
the contact torque vector, and □ indicates the left arm 
squeeze torque vector magnitude in GRL. FIG. 14 
35 shows that the coupler got snagged at the beginning of 
the task but broke free after applying approximately 6 
N-m. This snagging problem was also observed under 
dual-arm human control. Application of the dither input 
may help for this type of situation. As expected the rate 
40 of rotation decreased when the torque built up upon 
encountering resistance. The coupler could only rotate 
a finite amount, which was achieved after about 4 sec., 
when the torque increased to near its setpoint. The 
reason for the approximately 1 N-m error in steady state 
45 torque is unknown. 

XI.3 Dual-Arm Contour Following 

Partitioned dual-arm shared control was demon- 
strated with the dual-arm contour following task. The 
50 operator input motion in only three degrees of freedom 
of the task (TELEOP) frame, tangential to the surface 
and about the surface normal, thus preventing any mo- 
tion which may be damaging to the object. Autono- 
mous force control of 30N against the surface and force 
55 control about the tangential axes of the object caused 
the manipulators to automatically keep the flat tool 
surface tangential to the dome with a constant contact 
point in the center of the tool. The three DOFs for 
operator inputs were selected using a tpSelVect input 
60 vector of (1,1,0,0,0,1). The transform trRObj was a 
translation of — 120 mm along the Y axis. trGrR and 
trGrL were both translations of 335 mm along the Z 
axis. Squeeze control force control gains, sqFtGainsL 
and sqFtGainsR, were (0.01, 0.01, 0.01, 0.00001, 
65 0.00001, 0.00001) where translational gain units are 
mm/N and orientational gain units are deg/N-mm. The 
cfFtSetpoints input vector was (0,0, — 30N,0,0,0) and 
the cfFtGains input vector was (0.03, 0.03, 0.02, 



5,336,982 


21 

0.00008, 0.00008, 0.00001) where translational gain units 
are mm/N and orientational gain units are deg/N-mm. 
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tion and from the squeeze control position pertur- 
bation, new robot arm grasp points, and for con- 


XI.4 Dual-Arm Satellite Capture 

Shared control teleoperation was demonstrated in the 5 
dual-arm satellite capture task. The arms follow a ficti- 
tious common object whose motion is controlled by the 
operator with a hand controller. No squeeze control is 
used. The operator moves the one hand controller to 
make the arms capture the handles on the satellite as it 10 
rotates into the workspace. Force control of the contact 
(move) forces is used to damp the motion of the satellite 
after capture to cause it to stop moving. The transform 
trRObj was a translation of — 120 mm along the Y axis 
which was midway between the grippers. trGrR and 15 
trGrL were both translations of 225 mm long the Z axis. 
The cfFtGains input vector was (0.07, 0.07, 0.07, 
0.00004, 0.00004, 0.00004) where translational gain units 
are mm/N and orientational gain units are deg/N-mm. 
After tile satellite was captured, the grippers were 20 
closed on the satellite handles. 

In summary, the dual-arm generalized compliant 
motion task execution primitive provides cooperative 
dual-arm control task execution capability to a higher 
level planning system. The primitive provides multiple 25 
sensor based control including teleoperation which 
allows shared control execution. A unified algorithm 
for autonomous, teleoperation, and shared control is 
utilized to provide a wide range of task execution capa- 
bility. Description of the execution of various tasks 30 
using the primitive demonstrates the effectiveness of the 
method. 

While the invention has been described in detail by 
specific reference to preferred embodiments, it is under- 
stood that variations and modifications thereof may be 35 
made without departing from the true spirit and scope 
of the invention. 

What is claimed is: 

1. A multiple arm generalized compliant motion 
robot control system for governing a robot comprising 40 
dual multi-joint robot arms handling an object vrith 
both of said arms, comprising: 
means for defining plural input parameters governing 
plural respective behaviors to be exhibited by the 
robot in respective behavior spaces simultaneously; 45 
a move-squeeze decomposition processor means for 
computing actual move and squeeze decomposi- 
tion forces based upon current robot force sensor 
outputs; 

compliant motion processor means for transforming 50 
plural object position perturbations of said plural 
behaviors from the respective behavior spaces to a 
common space, and computing a relative transfor- 
mation to a behavior-commanded object position 
in accordance with the object position perturba- 55 
tions of said plural behaviors; 
kinematics processor means for updating a transfor- 
mation to a current commanded object position 
based upon the relative transformation to the 
behavior-commanded object position; 60 

multiple arm squeeze control processor means for 
computing, from appropriate squeeze force input 
parameters and from actual squeeze forces for each 
of the arms, a squeeze control position perturbation 


trolling respective joints of said robot arms in ac- 
cordance with said new robot arm grasp points. 

2. The system of claim 1 wherein: 

said plurality of input parameters comprises: Carte- 
sian, stiffness parameters, force setpoints, joint lim- 
its, joint singularities dither wave parameters, te- 
leoperation input frame of reference and a Carte- 
sian trajectory; and 

said plurality of behaviors comprises, respectively, 
Cartesian stiffness control, force control, joint limit 
avoidance, joint singularity avoidance, dither wave 
motion, teleoperation control and Cartesian trajec- 
tory control. 

3. The system of claim 2 wherein said compliant 
motion processor means comprises means for comput- 
ing position perturbations for all of said plural behaviors 
and combining all of said position perturbations. 

4. The system of claim 1 wherein one of said behav- 
iors comprises Cartesian trajectory control, said system 
further comprising: 

trajectory processor means for computing a drive 
transformation to a relative object position in ac- 
cordance with Cartesian trajectory input parame- 
ters; and 

wherein said kinematics processor means updates the 
transformation to a current commanded object 
position based upon the relative transformation to 
the behavior-commanded object position and based 
upon said drive transformation each sampling in- 
terval. 

5. The system of claim 4 wherein said drive transfor- 
mation is computed in accordance with input parame- 
ters specifying initial and destination object positions 
and in accordance with the number of elapsed sampling 
intervals. 

6. The system of claim 1 wherein: 

one of said input parameters comprises move force 
set points; 

said comphant motion processor means comprises 
force control means for computing a position per- 
turbation of said object in accordance with a prod- 
uct of a force control constant and a difference 
between said move force set points and said actual 
move forces. 

7. The system of claim 1 wherein: 

said input parameters comprise joint angle limits and 
joint angle singularities for in^vidual joints on said 
arms; 

said compliant motion processor means comprises 
means for computing for each of said arms a posi- 
tion perturbation of said object in accordance with 
a product of a force field constant and a reciprocal 
of a difference between actual joint angles sensed 
by joint angle sensors on said arms and correspond- 
ing ones of said joint angle limits and joint angle 
singularities. 

8. The system of claim 6 wherein said joint angle 
limits, said joint angle singularities and said force set 
points are comprised within a generahzed compliant 
motion primitive of user-specified input parameters. 

9. The system of claim 1 wherein said new robot arm 


for each of the arms, whereby to provide squeeze 65 grasp points are computed repetitively in successive 


control; and sample intervals and wherein said input parameters are 

inverse kinematics processor means for computing, changeable each sample interval whereby said pliural 
from the commanded object position transforma- behaviors are dynamically programmable. 
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10. The system of claim 1 wherein said plural behav- 
iors comprise at least one of safety monitoring and ter- 
mination condition monitoring, said system further 
comprising: 

means for stopping motion of said robot in response 5 
to predetermined quantities measured by predeter- 
mined sensors on said robot reaching predeter- 
mined values specified by corresponding ones of 
said input parameters. 

11. The system of claim 10 wherein: 10 

there are plural such predetermined quantities and 

said means for stopping stops said robot motion in 
response to all of said predetermined quantities 
meeting said predetermined values. 

12. A multiple arm generalized compliant motion IS 
robot control system for governing a robot comprising 
dual multi-joint robot arms handling an object with 
both of said arms, comprising: 

means for defining plural input parameters governing 
plural respective behaviors to be exhibited by the 20 
robot in respective behavior spaces simultaneously; 
a move-squeeze decomposition processor means for 
computing actual move and squeeze decomposi- 
tion forces based upon current robot force sensor 
outputs; 25 

compliant motion processor means for transforming 
plural object position perturbations of said plural 
behaviors from the respective behavior spaces to a 
common space, and computing a behavior-com- 
manded object position in accordance with the 30 
object position perturbations of said plural behav- 
iors; 

kinematics processor means for updating a current 
commanded object position based upon the behav- 
ior-connmanded object position; 35 

multiple arm squeeze control processor means for 
computing, from appropriate squeeze force input 
parameters and from actual squeeze forces for each 
of the arms, a squeeze control position perturbation 
for each of the arms, whereby to provide squeeze 40 
control; and 

inverse kinematics processor means for computing, 
from the commanded object position and from the 
squeeze control position perturbation, new robot 
arm grasp points and for controlling respective 45 
joints of said robot arms in accordance with said 
new robot arm grasp points. 

13. The system of claim 12 wherein: 

said plurality of input parameters comprises: Carte- 
sian stiffness parameters, force setpoints, joint lim- 50 
its, joint singularities, dither wave parameters, te- 
leoperation input frame of reference and a Carte- 
sian trajectory; and 

said plurdity of behaviors comprises, respectively, 
Cartesian stiffness control, force control, joint limit 55 
avoidance, joint singularity avoidance, dither wave 
motion, teleoperation control and Cartesian trajec- 
tory control. 

14. The system of claim 13 wherein said compliant 
motion processor means comprises means for comput- 60 
ing position perturbations for all of said pliural behaviors 
and combining all of said position perturbations. 

15. The system of claim 12 wherein one of said behav- 

iours comprises Cartesian trajectory control, said sys- 
tem further comprising: 65 

trajectory processor means for computing a relative 
object position in accordance with Cartesian tra- 
jectory input parameters; and 
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wherein said kinematics processor means updates the 
current commanded object position based upon the 
behavior-commanded object position and based 
upon said relative object position. 

16. The system of claim 15 wherein said relative ob- 
ject position is computed in accordance with input pa- 
rameters specifying initial and destination object posi- 
tions and m accordance with the number of elapsed 
sampling intervals. 

17. The system of claim 12 wherein: 

one of said input parameters comprises move force 
set points; 

said compliant motion processor means comprises 
force control means for computing a position per- 
turbation of said object in accordance with a prod- 
uct of a force control constant and a difference 
between said move force set points and said actual 
move forces. 

18. The system of claim 12 wherein: 

said input parameters comprise joint angle limits and 
joint an^e singularities for individual joints on said 
arms; 

said compliant motion processor means comprises 
means for computing for each of said arms a posi- 
tion perturbation of said object in accordance with 
a product of a force field constant and a reciprocal 
of a difference between actual joint angles sensed 
by joint angle sensors on said arms and correspond- 
ing ones of said joint angle limits and joint angle 
singularities. 

19. The system of claim 17 wherein said joint angle 
limits, said joint angle singularities and said force set 
points are comprised within a generalized compliant 
motion primitive of user-specified input parameters. 

20. The system of claim 12 wherein said new robot 
arm grasp points are computed repetitively in succes- 
sive sample intervals and wherein said input parameters 
are changeable each sample interval whereby said plu- 
ral behaviors are dynamically programmable. 

21. The system of claim 12 wherein said plmal behav- 
iors comprise at least one of safety monitoring and ter- 
mination condition monitoring, said system further 
comprising: 

means for stopping motion of said robot in response 
to predetermined quantities measured by predeter- 
mined sensors on said robot reaching predeter- 
mined values specified by corresponding ones of 
said input parameters. 

22. The system of claim 21 wherein: 

there are plural such predetermined quantities and 
said means for stopping stops said robot motion in 
response to all of said predetermined quantities 
meeting said predetermined values. 

23. A method for controlling a robot comprising dual 
multi-joint robot arms handling an object with both of 
said arms for multiple arm compliant motion, compris- 
ing: 

defining plural input parameters governing plural 
respective behaviors to be exhibited by the robot in 
respective behavior spaces simultaneously; 

first computing actual move and squeeze decomposi- 
tion forces based upon current robot force sensor 
outputs; 

first transforming plural object position perturbations 
of said plural behaviors from the respective behav- 
ior spaces to a common space, and second comput- 
ing a relative transformation to a behavior-com- 
manded object position in accordance with the 
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object position perturbations of said plural behav- 
iors; 

updating a transformation to a current commanded 
object position based upon the relative transforma- 
tion to the behavior-commanded object position; 

third computing, from appropriate squeeze force 
input parameters and from actual squeeze forces 
for each of the arms, a squeeze control position 
perturbation for each of the arms, whereby to pro- 
vide squeeze control; and 

fourth computing, from the commanded object posi- 
tion transformation and from the squeeze control 
position perturbation, new robot arm grasp points, 
and controlling respective joints of said robot arms 
in accordance with said new robot joint angles. 

24. The method of claim 23 wherein: 

said plurality of input parameters comprises: Carte- 
sian stiffness parameters, force setpoints, joint lim- 
its, joint singularities, dither wave parameters, te- 
leoperation input frame of reference and at Carte- 
sian trajectory; and 

smd plurality of behaviors comprises, respectively, 
Cartesian stiffness control, force control, joint limit 
avoidance, joint singularity avoidance, dither wave 
motion, teleoperation control and Cartesian trajec- 
tory control. 

25. TThe method of claim 24 wherein said second 
computing comprises computing position perturbations 
for all of said plural behaviors and combining all of said 
position perturbations. 

26. The method of claim 23 wherein one of said be- 
haviors comprises Cartesian trajectory control, said 
method further comprising: 

fifth computing a drive transformation to a relative 
object position in accordance with Cartesian tra- 
jectory input parameters; and 

wherein said updating updates the transformation to a 
current commanded object position based upon the 
relative transformation to the behavior-com- 
manded object position and based upon said drive 
transformation each sampling interval. 

27. The method of claim 26 wherein said drive trans- 
formation is computed in accordance with input param- 
eters specifying initial and destination object positions 
and in accordance with the number of elapsed sampling 
intervals. 

28. The method of claim 23 wherein: 

one of said input parameters comprises move force 
set points; 

said second computing comprises computing a posi- 
tion perturbation of said object in accordance with 
a product of a force control constant and a differ- 
ence between said move force set points and said 
actual move forces. 

29. The method of claim 23 wherein: 

said input parameters comprise joint angle limits and 
joint angle singularities for individual joints on said 


26 

points are comprised within a generalized compliant 
motion primitive of user-specified input parameters. 

31. The method of claim 23 wherein said new robot 
arm grasp points are computed repetitively in succes- 

5 sive sample intervals, said method further comprising 
changing selected ones of said input parameters each 
sample interval whereby to dynamically program said 
plural behaviors. 

32. The method of claim 23 wherein said plural be- 
10 haviors comprise at least one of safety monitoring and 

termination condition monitoring, said method further 
comprising: 

stopping motion of said robot in response to predeter- 
mined quantities measured by predetermined sen- 
15 sors on said robot reaching predetermined values 
specified by corresponding ones of said input pa- 
rameters. 

33. The method of claim 32 wherein: 

there are plural such predetermined quantities and 
20 said stopping stops said robot motion in response to 
all of said predetermined quantities meeting said 
predetermined values. 

34. A method for controlling a robot comprising dual 
multi-joint robot arms handling an object with both of 

25 said arms for multiple arm compliant motion, compris- 
ing: 

defining plural input parameters governing plural 
respective behaviors to be exhibited by the robot in 
respective behavior spaces simultaneously; 

30 first computing actual move and squeeze decomposi- 
tion forces based upon current robot force sensor 
outputs; 

first transfonning plural object position perturbations 
of said plural behaviors from the respective behav- 
35 ior spaces to a common space, and second comput- 
ing a behavior-commanded object position in ac- 
cordance with the object position perturbations of 
said plural behaviors; 

updating a current commanded object position based 
40 upon the behavior-commanded object position; 

third computing, from appropriate squeeze force 
input parameters and from actual squeeze forces 
for each of the arms, a squeeze control position 
perturbation for each of the arms, whereby to pro- 
45 vide squeeze control; and 

fourth computing, from the commanded object posi- 
tion and from the squeeze control position pertur- 
bation, new robot arm grasp points, and controlling 
respective joints of said robot arms in accordance 
50 with said new robot arm grasp points. 

35. The method of claim 34 wherein: 

said plurality of input parameters comprises: Carte- 
sian stiffness parameters, force setpoints, joint lim- 
its, joint singularities, dither wave parameters, te- 
55 leoperation input frame of reference and a Carte- 
sian trajectory; and 

said plur^ty of behaviors comprises, respectively, 
Cartesian stiffness control, force control, joint limit 


arms; avoidance, joint singularity avoidance, dither wave 

said second computing comprises computing for each 60 motion, teleoperation control and Cartesian trajec- 
of said arms a position perturbation of said object in tory control. 

accordance with a product of a force field constant 36. TTie method of claim 35 wherein said second 


and a reciprocal of a difference between actual 
joint angles sensed by joint angle sensors on said 
arms and corresponding ones of said joint angle 
limits and joint angle singularities. 

30. The method of claim 28 wherein said joint angle 
limits, said joint angle singularities and said force set 


computing comprises computing position perturbations 
for aU of said plural behaviors and combining all of said 
65 position perturbations. 

37. The method of claim 34 wherein one of said be- 
haviors comprises Cartesian trajectory control, said 
method further comprising: 



5,336,982 


27 

fifth computing a relative object position in accor- 
dance with Cartesian trajectory input parameters; 
and 

wherein said updating updates the current com- 
manded object position based upon the behavior- 
commanded object position and based upon said 
relative object position. 

38. The me^od of claim 37 wherein said relative 
object position is computed in accordance with input 
parameters specifying initial and destination object posi- 
tions and in accordance with the number of elapsed 
sampling intervals. 

39. The method of claim 34 wherein; 

one of said input parameters comprises move force 

set points. 

said second computing comprises computing a posi- 
tion perturbation of said object in accordance with 
a product of a force control constant and a differ- 
ence between said move force set points and said 
actual move forces. 

40. The method of claim 34 wherein: 

said input parameters comprise joint angle limits and 

joint angle singularities for individual joints on said 
arms; 

said second computing comprises computing for each 25 
of said arms a position perturbation of said object in 
accordance with a product of a force field constant 
and a reciprocal of a difference between actual 
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joint angles sensed by joint angle sensors on said 
arms and corresponding ones of said joint angle 
limits and joint angle singularities. 

41. The method of claim 39 wherein said joint angle 
5 limits, said joint angle singularities and said force set 

points are comprised within a generalized compliant 
motion primitive of user-specified input parameters. 

42. The method of claim 34 wherein said new robot 
arm grasp points are computed repetitively in succes- 

10 sive sample intervals and wherein said method further 
comprises changing selected ones of said input parame- 
ters each sample interval whereby to dynamically pro- 
gram said plural behaviors. 

43. The method of claim 34 wherein said plural be- 
15 haviors comprise at least one of safety monitoring and 

termination condition monitoring, said method further 
comprising: 

stopping motion of said robot in response to predeter- 
mined quantities measured by predetermmed sen- 
20 sors on said robot reaching predetermined values 
specified by corresponding ones of said input pa- 
rameters. 

44. The method of claim 43 wherein: 

there are plural such predetermined quantities and 
said stopping stops said robot motion in response to 
all of said predetermined quantities meeting said 
predetermined values. 
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